//
// Created by lqk on 2022/1/24.
//


#ifndef QUADCEREBELLUM_GAZEBOMOTORDRIVER_H
#define QUADCEREBELLUM_GAZEBOMOTORDRIVER_H
#include "rosbridge_ws_client.hpp"
#include "PeriodicTask.h"
#include "PeriodicTaskManager.h"
#include "MotorsStructs.h"
#include "PyBulletMotorsDriver.h"




class GazeboMotorsDriver : public PeriodicTask {
public:
    GazeboMotorsDriver(PeriodicTaskManager* taskManager,const float& period,const std::string& name,const std::string& port="localhost:9099"): PeriodicTask(taskManager,period,name),m_rbc(port){}
    virtual void Init() override;
    virtual void RunOnce() override;
private:
    void SendCommand();
    virtual void CleanUp() override;
    void ActuatorSimulate();

//    MotorsCommand* const m_motorsCommand;
//    MotorsInfo* const m_motorsInfo;
    float motorsTorque[12]{};
    RosbridgeWsClient m_rbc;
    std::vector<double> v_torque;

    static void SubscribeCallback(std::shared_ptr<WsClient::Connection>, std::shared_ptr<WsClient::InMessage>);

};


#endif //QUADCEREBELLUM_GAZEBOMOTORDRIVER_H
